//
// Created by jimmyteng on 19-4-10.
//
#include <opencv2/opencv.hpp>
#include "visual_compass/visual_compass.h"

int main() {
  std::string filename = ("../resource/testdata/1.png");//set Data path
  cv::Mat image = cv::imread(filename);
  if (image.empty()) return -1;

  //Initialize parameter of camera
  float r[9]{-8.08247458e-03, 9.99967098e-01, 6.55866519e-04,
             -9.99333203e-01, -8.05399381e-03, -3.56122330e-02,
             -3.56057808e-02, -9.43264225e-04, 9.99365449e-01
  };

  float camera_matrix[9] = {2.79109619e+02, 0., 3.16118042e+02,
                            0., 2.79484833e+02, 2.37545059e+02,
                            0., 0., 1.};
  float distortion_coefficients[4] = {-7.02879429e-02, 2.03178778e-01, -2.42307484e-01,
                                      1.02940999e-01};


  cv::Mat K(3, 3, CV_32F, camera_matrix);
  cv::Mat DistCoef(1, 4, CV_32F, distortion_coefficients);
  cv::Mat R(3, 3, CV_32F, r);
  //Initialize
  VisualCompass::GetInstance()->Initialize(K, DistCoef, R);
  double res, score;
  VisualCompass::GetInstance()->Detect(image, res, score);
  std::cout << "Res Degree = " << res << " with score of " << score << std::endl;
  return 0;
}